
import time
from threading import Thread, Lock
from msvcrt import getch

from Brain.arm.arm import Arm
from Brain.plan import BuildStep
from Brain.arm.robot_arm import ArmControl

class KeyInfo:
	def __init__(self, **kw):
		self.key = ''
		self.processed = False
		self.__dict__.update(kw)

class Testing:
	ANGLE_INCREMENT = 5
	START_POSITION = 'store-start'
	
	def __init__(self, arm):
		self._commands = []
		self._keyboard = Thread(target=self.keyboard_thread)
		self._keyboard.setDaemon(True)
		self._keyboard.start()
		self._key_resource = Lock()
		self.arm = arm
	
	def keyboard_thread(self):
		while 1:
			ch = getch()
			self._key_resource.acquire()
			self._commands.append( KeyInfo(key=ch) )
			self._key_resource.release()
			time.sleep(0.001)
	
	def run(self):
		while 1:
			self._key_resource.acquire()
			commands = self._commands
			self._commands = []
			self._key_resource.release()
			
			for command in commands:
				# process key
				key = command.key 
				if key == 'q': self.arm.move_incremental(0, self.ANGLE_INCREMENT, 12)
				elif key == 'a': self.arm.move_incremental(0, -self.ANGLE_INCREMENT, 12)
				elif key == 'w': self.arm.move_incremental(1, self.ANGLE_INCREMENT, 12)
				elif key == 's': self.arm.move_incremental(1, -self.ANGLE_INCREMENT, 12)
				elif key == 'e': self.arm.move_incremental(2, self.ANGLE_INCREMENT, 12)
				elif key == 'd': self.arm.move_incremental(2, -self.ANGLE_INCREMENT, 12)
				elif key == 'r': self.arm.move_incremental(3, self.ANGLE_INCREMENT, 12)
				elif key == 'f': self.arm.move_incremental(3, -self.ANGLE_INCREMENT, 12)
				elif key == 't': self.arm.move_incremental(4, self.ANGLE_INCREMENT, 12)
				elif key == 'g': self.arm.move_incremental(4, -self.ANGLE_INCREMENT, 12)
				elif key == 'y': self.arm.move_incremental(5, self.ANGLE_INCREMENT, 12)
				elif key == 'h': self.arm.move_incremental(5, -self.ANGLE_INCREMENT, 12)
				elif key == '`': self.arm.move_to_position(self.START_POSITION, 12)
				
				elif key == '3': 
					step = BuildStep(12.0, 10,0,5)
					self.arm.start_task( step )
				
				elif key == '1': 
					self.arm. simulate_external_signal(True)
					
				elif key == '2': 
					self.arm. simulate_external_signal(False)
				
				elif key == '4':
					self.arm.set_raw_position( [793, 763, 887, 416, 795, 446]) 
				
				elif key == '5':
					self.arm.set_raw_position([788, 803, 892, 316, 790, 426]) 
				
				
				elif key == '\r':
					print '\n\n\t %s \n' % (self.arm._joints, )
				elif key == '\x1b':
					import sys
					sys.exit()
				else : 
					print 'key: %r' % (key, )
				break
			time.sleep(0.01)

if __name__ == '__main__':
	arm = ArmControl()
	arm.start()
	t = Testing(arm)
	t.run()
	





